
#include "lidar.hpp"

#include "chassic.hpp"
#include "gtest/gtest.h"
#include "log.hpp"
#include "spdlog/spdlog.h"

// TEST(TestDevice, TestScan) {
//   component::Logger::SetLogger("logs/lidar.log");
//   LidarDriver lidar;
//   // lidar.SetLimit(160, 200);
//   lidar.Init();
//   while (true) {
//     auto f = lidar.Apply();
//     std::string a;
//     for (auto& ff : f) {
//       a += (std::to_string(ff) + ',');
//     }
//     SPDLOG_WARN("{}", a);
//   }
//   EXPECT_EQ(1, 1);
// }

// TEST(TestDevice, TestAlgo) {
//   component::Logger::SetLogger("logs/algo.log");

//   std::vector<component::LidarPoint> inners_;
//   std::vector<float> measured_;
//   component::LidarPoint p;

//   measured_.resize(720, 0);

//   for (int i = 0; i < 3710 - 1; i++) {
//     p.angle_ = float(i) / 3710 * 360;
//     p.dist_ = i % 150;
//     // SPDLOG_WARN("{}, {}", p.angle_, p.dist_);
//     inners_.emplace_back(p);
//   }

//   SPDLOG_WARN("inited");

//   // auto h = inners_.back();
//   // SPDLOG_WARN("{}, {}", h.angle_, h.dist_);

//   for (auto i = inners_.size() - 1; i > 0; i--) {
//     SPDLOG_WARN("{}, {}", inners_[i].angle_, inners_[i].dist_);

//     if (inners_[i].angle_ > 359.75)
//       inners_[i].angle_ -= 360;
//     else
//       break;
//     SPDLOG_WARN("{}, {}", inners_[i].angle_, inners_[i].dist_);
//   }
//   int all_count = 0;
//   for (std::size_t angle_index = 0; angle_index < 720; angle_index++) {
//     int count = 0;
//     float sum = 0;

//     for (std::size_t element_index = 1; element_index < inners_.size();
//          element_index++) {
//       all_count++;
//       if (inners_[element_index].angle_ < (angle_index * 0.5 + 0.25) &&
//           inners_[element_index].angle_ > (angle_index * 0.5 - 0.25)) {
//         count++;
//         sum += inners_[element_index].dist_;
//         SPDLOG_WARN("element_index {}, angle_index {}. count{}, sum{}",
//                     element_index, angle_index, count, sum);
//       } else {
//         measured_[angle_index] = sum / count;
//         angle_index++;
//         count = 0;
//         sum = 0;
//       }
//     }
//   }
//   std::string a;
//   for (auto& ff : measured_) {
//     a += (std::to_string(ff) + ',');
//   }
//   SPDLOG_WARN("{}", a);
//   SPDLOG_WARN("{}", all_count);
//   SPDLOG_WARN("{}", measured_.size());
// }